


#include "isp.h"



#define SPC_LOGNAME         "spc"
#define spc_debug(...)      qelog_debug(ISP_LOGNAME,  __VA_ARGS__)
#define spc_info(...)       qelog_info(ISP_LOGNAME,   __VA_ARGS__)
#define spc_notice(...)     qelog_notice(ISP_LOGNAME, __VA_ARGS__)
#define spc_warning(...)    qelog_warning(ISP_LOGNAME,   __VA_ARGS__)
#define spc_error(...)      qelog_error(ISP_LOGNAME,  __VA_ARGS__)
#define spc_fatal(...)      qelog_fatal(ISP_LOGNAME,    __VA_ARGS__)



qe_ret spc_calc(void *frames, int num_frames, qe_dim2 dim, qe_box box, void *expaddr)
{
    char *cache_buf;
    qe_u32 ts, t1, t2;
    qe_u32 *p, *p_avg, *p_ofs;
    qe_u32 *cache, *cache_temp;
    qe_u32 bytes_per_cache, cache_size;
    qe_u32 pix_per_box;
    qe_u32 pix_per_frame;
    qe_u32 pix_per_cache;
    qe_u32 average;
    qe_u32 mean;
    qe_u64 sum;

    if (!frames || !num_frames || !expaddr)
        return qe_err_param;

    /*
     * p     : pointer to input frames
     * p_avg : pointer to average image
     * p_ofs : pointer to offset image
     */

    
    p               = (qe_u32 *)frames;
    p_avg           = (qe_u32 *)frames;
    p_ofs           = (qe_u32 *)expaddr;
    sum             = 0;
    average         = 0;
    pix_per_frame   = dim.x * dim.y;
    pix_per_cache   = dim.x * SPC_CC_LINES;
    pix_per_box     = box.w * box.h;
    bytes_per_cache = sizeof(qe_u32) * pix_per_cache;
    cache_size      = bytes_per_cache * 2;

    cache_buf = qe_malloc(cache_size);
    qe_assert(cache_buf != QE_NULL);
    cache = (qe_u32 *)cache_buf;
    cache_temp = cache + pix_per_cache;

    ts = qe_time_ms();
    t1 = ts;
    isp_info("spc calc step1 start...");

    qe_memset(cache_buf, 0x0, cache_size);

    for (int j=box.y; j<(box.y+box.h); j+=SPC_CC_LINES) {

        /* clean cache */
        qe_memset(cache, 0x0, bytes_per_cache);

        for (int i=0; i<num_frames; i++) {

            /* copy rows to cache */
            qe_memcpy(cache_temp, &p[pix_per_frame * i + dim.x * j], bytes_per_cache);

            for (int n=0; n<SPC_CC_LINES; n++) {
                for (int k=box.x; k<(box.x+box.w); k++) {
                    cache[dim.x * n + k] += cache_temp[dim.x * n + k];
                    sum += cache_temp[dim.x * n + k];
                }
            }
        }
        qe_memcpy(&p_avg[dim.x * j], cache, bytes_per_cache);
    }

    isp_debug("sum:%ld", sum);
    qe_u32 div = pix_per_box * (qe_u32)num_frames;
    isp_debug("div:%d", div);
    average = (qe_u32)(sum / div);
    isp_info("average:%d", average);
    t2 = qe_time_ms();
    isp_info("spc calc step1 finish in %dms", t2-t1);

    isp_info("spc calc step2 start...", t2-t1);
    t1 = qe_time_ms();

    for (int j=box.y; j<(box.y+box.h); j+=SPC_CC_LINES) {

        qe_memcpy(cache, &p_avg[dim.x * j], bytes_per_cache);

        for (int n=0; n<SPC_CC_LINES; n++) {
            for (int k=box.x; k<(box.x+box.w); k++) {
                mean = cache[dim.x * n + k] / num_frames;  /* don't forget average image now is sum */
                cache_temp[dim.x * n + k] = (mean >= average) ? (mean - average) : 
                       (1<<31) | (average - mean);
            }
        }
        
        qe_memcpy(&p_ofs[dim.x * j], cache_temp, bytes_per_cache);
    }

    t2 = qe_time_ms();
    isp_info("spc calc step2 finish in %dms", t2-t1);
    isp_info("spc calc runtime %dms", t2-ts);

    qe_free(cache_buf);

    return qe_ok;
}
